#include "bsp_PCA9685_0.h"
#include "myiic_0.h"
#include "delay.h"
#include <math.h>
#include "main.h"

void PCA9685_Init_0(float hz,u8 angle)
{
	u32 off = 310;
	PCA9685_Write_0(PCA_Model,0x00);
	PCA9685_setFreq_0(hz);
	//off = (u32)(145+angle*2.4);
	PCA9685_setPWM_0(0,0,off);
	PCA9685_setPWM_0(1,0,off);
	PCA9685_setPWM_0(2,0,off);
	PCA9685_setPWM_0(3,0,off);
	PCA9685_setPWM_0(4,0,off);
	PCA9685_setPWM_0(5,0,off);
	PCA9685_setPWM_0(6,0,off);
	PCA9685_setPWM_0(7,0,off);
	PCA9685_setPWM_0(8,0,off);
	PCA9685_setPWM_0(9,0,off);
	PCA9685_setPWM_0(10,0,off);
	PCA9685_setPWM_0(11,0,off);
	PCA9685_setPWM_0(12,0,off);
	PCA9685_setPWM_0(13,0,off);
	PCA9685_setPWM_0(14,0,off);
	PCA9685_setPWM_0(15,0,off);

	PCA_FLAG_2 = 1;
	HAL_Delay(100);
	
}

void PCA9685_Write_0(u8 addr,u8 data)
{
	IIC_Start_0();
	
	IIC_Send_Byte_0(PCA_Addr);
	IIC_NAck_0();
	
	IIC_Send_Byte_0(addr);
	IIC_NAck_0();
	
	IIC_Send_Byte_0(data);
	IIC_NAck_0();
	
	IIC_Stop_0();
	
	
}

u8 PCA9685_Read_0(u8 addr)
{
	u8 data;
	
	IIC_Start_0();
	
	IIC_Send_Byte_0(PCA_Addr);
	IIC_NAck_0();
	
	IIC_Send_Byte_0(addr);
	IIC_NAck_0();
	
	IIC_Stop_0();
	
	delay_us(10);

	
	IIC_Start_0();

	IIC_Send_Byte_0(PCA_Addr|0x01);
	IIC_NAck_0();
	
	data = IIC_Read_Byte_0(0);
	
	IIC_Stop_0();
	
	return data;
	
}

void PCA9685_setPWM_0(u8 num,u32 on,u32 off)
{
	IIC_Start_0();
	
	IIC_Send_Byte_0(PCA_Addr);
	IIC_Wait_Ack_0();
	
	IIC_Send_Byte_0(LED0_ON_L+4*num);
	IIC_Wait_Ack_0();
	
	IIC_Send_Byte_0(on&0xFF);
	IIC_Wait_Ack_0();
	
	IIC_Send_Byte_0(on>>8);
	IIC_Wait_Ack_0();
	
	IIC_Send_Byte_0(off&0xFF);
	IIC_Wait_Ack_0();
	
	IIC_Send_Byte_0(off>>8);
	IIC_Wait_Ack_0();
	
	IIC_Stop_0();
	
}

void PCA9685_setFreq_0(float freq)
{
	u8 prescale,oldmode,newmode;
	
	double prescaleval;
	
	//freq *= 0.92;
	prescaleval = 25000000;
	prescaleval /= 4096;
	prescaleval /= freq;
	prescaleval -= 1;
	prescale = floor(prescaleval+0.5f);
	oldmode = PCA9685_Read_0(PCA_Model);
	
	newmode = (oldmode&0x7F)|0x10;
	PCA9685_Write_0(PCA_Model,newmode);
	PCA9685_Write_0(PCA_Pre,prescale);
	PCA9685_Write_0(PCA_Model,oldmode);
	HAL_Delay(5);
	PCA9685_Write_0(PCA_Model,oldmode|0xa1);
	
	
}

void setAngle_0(u8 num,u8 angle)
{
	u32 off = 0;
	off = (u32)(158+angle*2.2);
	PCA9685_setPWM_0(num,0,off);
}



